#include <robot_chassis/CRobotOdom.hpp>
#include <cmath>

using namespace std::chrono_literals;

CRobotOdometry::CRobotOdometry(const std::string& nodeName)
    : Node(nodeName), turnonRobot_(CTurnOnRobot::getInstance())
{
    // 声明参数
    this->declare_parameter<std::string>("odom_frame_id", "odom");
    this->declare_parameter<std::string>("base_frame_id", "base_link");
    this->declare_parameter<bool>("publish_tf", true);

    // 获取参数
    odomFrameId_ = this->get_parameter("odom_frame_id").as_string();
    baseFrameId_ = this->get_parameter("base_frame_id").as_string();
    publishTf_   = this->get_parameter("publish_tf").as_bool();

    // 创建发布者
    odomPub_ = this->create_publisher<nav_msgs::msg::Odometry>("chassis/odom", 10);

    // 创建TF广播器
    tfBroadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // 初始化时间
    lastTime_ = this->now();

    // 创建定时器，100Hz
    timer_ = this->create_wall_timer(10ms, std::bind(&CRobotOdometry::callback, this));

    RCLCPP_INFO(this->get_logger(), "里程计节点已启动 - odom_frame: %s, base_frame: %s",
                odomFrameId_.c_str(), baseFrameId_.c_str());
}

CRobotOdometry::~CRobotOdometry() {}

void CRobotOdometry::computeOdometry()
{
    // 获取速度数据
    turnonRobot_.getOdomData(velocityData_);

    // 计算时间差
    rclcpp::Time currentTime = this->now();
    double       dt          = (currentTime - lastTime_).seconds();
    lastTime_                = currentTime;

    // 防止时间差过大
    if (dt > 0.5 || dt <= 0.0) {
        dt = 0.01;
    }

    // 获取速度
    double vx = velocityData_.vxData_;  // X轴速度 (m/s)
    double vy = velocityData_.vyData_;  // Y轴速度 (m/s)
    double vz = velocityData_.vzData_;  // 角速度 (rad/s)

    // 里程计积分 - 使用中点积分法
    double deltaTheta = vz * dt;
    double deltaX, deltaY;

    if (std::abs(vz) < 1e-6) {
        // 直线运动
        deltaX = vx * dt;
        deltaY = vy * dt;
    } else {
        // 圆弧运动
        deltaX = (vx * std::sin(deltaTheta) + vy * (std::cos(deltaTheta) - 1.0)) / vz;
        deltaY = (vx * (1.0 - std::cos(deltaTheta)) + vy * std::sin(deltaTheta)) / vz;
    }

    // 更新位姿 (全局坐标系)
    posX_  += deltaX * std::cos(theta_) - deltaY * std::sin(theta_);
    posY_  += deltaX * std::sin(theta_) + deltaY * std::cos(theta_);
    theta_ += deltaTheta;

    // 归一化角度到 [-π, π]
    while (theta_ > M_PI) theta_ -= 2.0 * M_PI;
    while (theta_ < -M_PI) theta_ += 2.0 * M_PI;

    // 填充里程计消息
    odomMsg_.header.stamp    = currentTime;
    odomMsg_.header.frame_id = odomFrameId_;
    odomMsg_.child_frame_id  = baseFrameId_;

    // 位姿
    odomMsg_.pose.pose.position.x = posX_;
    odomMsg_.pose.pose.position.y = posY_;
    odomMsg_.pose.pose.position.z = 0.0;

    // 四元数
    tf2::Quaternion q;
    q.setRPY(0, 0, theta_);
    odomMsg_.pose.pose.orientation.x = q.x();
    odomMsg_.pose.pose.orientation.y = q.y();
    odomMsg_.pose.pose.orientation.z = q.z();
    odomMsg_.pose.pose.orientation.w = q.w();

    // 速度 (在base_link坐标系下)
    odomMsg_.twist.twist.linear.x  = vx;
    odomMsg_.twist.twist.linear.y  = vy;
    odomMsg_.twist.twist.linear.z  = 0.0;
    odomMsg_.twist.twist.angular.x = 0.0;
    odomMsg_.twist.twist.angular.y = 0.0;
    odomMsg_.twist.twist.angular.z = vz;

    // 协方差矩阵
    odomMsg_.pose.covariance = {
        1e-3, 0,    0,   0,   0,    0,
        0,    1e-3, 0,   0,   0,    0,
        0,    0,    1e6, 0,   0,    0,
        0,    0,    0,   1e6, 0,    0,
        0,    0,    0,   0,   1e6,  0,
        0,    0,    0,   0,   0,    1e-3
    };

    odomMsg_.twist.covariance = {
        1e-3, 0,    0,   0,   0,    0,
        0,    1e-3, 0,   0,   0,    0,
        0,    0,    1e6, 0,   0,    0,
        0,    0,    0,   1e6, 0,    0,
        0,    0,    0,   0,   1e6,  0,
        0,    0,    0,   0,   0,    1e-3
    };
}

void CRobotOdometry::publishOdometry()
{
    odomPub_->publish(odomMsg_);
}

void CRobotOdometry::publishTF()
{
    if (!publishTf_) {
        return;
    }

    geometry_msgs::msg::TransformStamped transformStamped;

    transformStamped.header.stamp    = odomMsg_.header.stamp;
    transformStamped.header.frame_id = odomFrameId_;
    transformStamped.child_frame_id  = baseFrameId_;

    transformStamped.transform.translation.x = posX_;
    transformStamped.transform.translation.y = posY_;
    transformStamped.transform.translation.z = 0.0;

    transformStamped.transform.rotation = odomMsg_.pose.pose.orientation;

    tfBroadcaster_->sendTransform(transformStamped);
}

void CRobotOdometry::callback()
{
    computeOdometry();
    publishOdometry();
    publishTF();
}
